home *** CD-ROM | disk | FTP | other *** search
/ Amiga Plus 2000 #5 / Amiga Plus CD - 2000 - No. 5.iso / Tools / Dev / lame_src / fft.c < prev    next >
C/C++ Source or Header  |  2000-01-01  |  10KB  |  357 lines
  1. /*
  2. ** FFT and FHT routines
  3. **  Copyright 1988, 1993; Ron Mayer
  4. **  
  5. **  fht(fz,n);
  6. **      Does a hartley transform of "n" points in the array "fz".
  7. **      
  8. ** NOTE: This routine uses at least 2 patented algorithms, and may be
  9. **       under the restrictions of a bunch of different organizations.
  10. **       Although I wrote it completely myself; it is kind of a derivative
  11. **       of a routine I once authored and released under the GPL, so it
  12. **       may fall under the free software foundation's restrictions;
  13. **       it was worked on as a Stanford Univ project, so they claim
  14. **       some rights to it; it was further optimized at work here, so
  15. **       I think this company claims parts of it.  The patents are
  16. **       held by R. Bracewell (the FHT algorithm) and O. Buneman (the
  17. **       trig generator), both at Stanford Univ.
  18. **       If it were up to me, I'd say go do whatever you want with it;
  19. **       but it would be polite to give credit to the following people
  20. **       if you use this anywhere:
  21. **           Euler     - probable inventor of the fourier transform.
  22. **           Gauss     - probable inventor of the FFT.
  23. **           Hartley   - probable inventor of the hartley transform.
  24. **           Buneman   - for a really cool trig generator
  25. **           Mayer(me) - for authoring this particular version and
  26. **                       including all the optimizations in one package.
  27. **       Thanks,
  28. **       Ron Mayer; mayer@acuson.com
  29. ** and added some optimization by
  30. **           Mather    - idea of using lookup table
  31. **           Takehiro  - some dirty hack for speed up
  32. */
  33.  
  34. #include <math.h>
  35. #include "util.h"
  36. #include "psymodel.h"
  37. #include "lame.h"
  38.  
  39. #define TRI_SIZE (5-1) /* 1024 =  4**5 */
  40. static FLOAT costab[TRI_SIZE*2];
  41. static FLOAT window[BLKSIZE / 2], window_s[BLKSIZE_s / 2];
  42.  
  43. static INLINE void fht(FLOAT *fz, int n)
  44. {
  45.     short k4;
  46.     FLOAT *fi, *fn, *gi;
  47.     FLOAT *tri;
  48.  
  49.     fn = fz + n;
  50.     tri = &costab[0];
  51.     k4 = 4;
  52.     do {
  53.     FLOAT s1, c1;
  54.     short i, k1, k2, k3, kx;
  55.     kx  = k4 >> 1;
  56.     k1  = k4;
  57.     k2  = k4 << 1;
  58.     k3  = k2 + k1;
  59.     k4  = k2 << 1;
  60.     fi  = fz;
  61.     gi  = fi + kx;
  62.     do {
  63.         FLOAT f0,f1,f2,f3;
  64.         f1      = fi[0]  - fi[k1];
  65.         f0      = fi[0]  + fi[k1];
  66.         f3      = fi[k2] - fi[k3];
  67.         f2      = fi[k2] + fi[k3];
  68.         fi[k2]  = f0     - f2;
  69.         fi[0 ]  = f0     + f2;
  70.         fi[k3]  = f1     - f3;
  71.         fi[k1]  = f1     + f3;
  72.         f1      = gi[0]  - gi[k1];
  73.         f0      = gi[0]  + gi[k1];
  74.         f3      = SQRT2  * gi[k3];
  75.         f2      = SQRT2  * gi[k2];
  76.         gi[k2]  = f0     - f2;
  77.         gi[0 ]  = f0     + f2;
  78.         gi[k3]  = f1     - f3;
  79.         gi[k1]  = f1     + f3;
  80.         gi     += k4;
  81.         fi     += k4;
  82.     } while (fi<fn);
  83.     c1 = tri[0];
  84.     s1 = tri[1];
  85.     for (i = 1; i < kx; i++) {
  86.         FLOAT c2,s2;
  87.         c2 = 1 - (2*s1)*s1;
  88.         s2 = (2*s1)*c1;
  89.         fi = fz + i;
  90.         gi = fz + k1 - i;
  91.         do {
  92.         FLOAT a,b,g0,f0,f1,g1,f2,g2,f3,g3;
  93.         b       = s2*fi[k1] - c2*gi[k1];
  94.         a       = c2*fi[k1] + s2*gi[k1];
  95.         f1      = fi[0 ]    - a;
  96.         f0      = fi[0 ]    + a;
  97.         g1      = gi[0 ]    - b;
  98.         g0      = gi[0 ]    + b;
  99.         b       = s2*fi[k3] - c2*gi[k3];
  100.         a       = c2*fi[k3] + s2*gi[k3];
  101.         f3      = fi[k2]    - a;
  102.         f2      = fi[k2]    + a;
  103.         g3      = gi[k2]    - b;
  104.         g2      = gi[k2]    + b;
  105.         b       = s1*f2     - c1*g3;
  106.         a       = c1*f2     + s1*g3;
  107.         fi[k2]  = f0        - a;
  108.         fi[0 ]  = f0        + a;
  109.         gi[k3]  = g1        - b;
  110.         gi[k1]  = g1        + b;
  111.         b       = c1*g2     - s1*f3;
  112.         a       = s1*g2     + c1*f3;
  113.         gi[k2]  = g0        - a;
  114.         gi[0 ]  = g0        + a;
  115.         fi[k3]  = f1        - b;
  116.         fi[k1]  = f1        + b;
  117.         gi     += k4;
  118.         fi     += k4;
  119.         } while (fi<fn);
  120.         c2 = c1;
  121.         c1 = c2 * tri[0] - s1 * tri[1];
  122.         s1 = c2 * tri[1] + s1 * tri[0];
  123.         }
  124.     tri += 2;
  125.     } while (k4<n);
  126. }
  127.  
  128. static const short rv_tbl[] = {
  129.     0x00,    0x80,    0x40,    0xc0,    0x20,    0xa0,    0x60,    0xe0,
  130.     0x10,    0x90,    0x50,    0xd0,    0x30,    0xb0,    0x70,    0xf0,
  131.     0x08,    0x88,    0x48,    0xc8,    0x28,    0xa8,    0x68,    0xe8,
  132.     0x18,    0x98,    0x58,    0xd8,    0x38,    0xb8,    0x78,    0xf8,
  133.     0x04,    0x84,    0x44,    0xc4,    0x24,    0xa4,    0x64,    0xe4,
  134.     0x14,    0x94,    0x54,    0xd4,    0x34,    0xb4,    0x74,    0xf4,
  135.     0x0c,    0x8c,    0x4c,    0xcc,    0x2c,    0xac,    0x6c,    0xec,
  136.     0x1c,    0x9c,    0x5c,    0xdc,    0x3c,    0xbc,    0x7c,    0xfc,
  137.     0x02,    0x82,    0x42,    0xc2,    0x22,    0xa2,    0x62,    0xe2,
  138.     0x12,    0x92,    0x52,    0xd2,    0x32,    0xb2,    0x72,    0xf2,
  139.     0x0a,    0x8a,    0x4a,    0xca,    0x2a,    0xaa,    0x6a,    0xea,
  140.     0x1a,    0x9a,    0x5a,    0xda,    0x3a,    0xba,    0x7a,    0xfa,
  141.     0x06,    0x86,    0x46,    0xc6,    0x26,    0xa6,    0x66,    0xe6,
  142.     0x16,    0x96,    0x56,    0xd6,    0x36,    0xb6,    0x76,    0xf6,
  143.     0x0e,    0x8e,    0x4e,    0xce,    0x2e,    0xae,    0x6e,    0xee,
  144.     0x1e,    0x9e,    0x5e,    0xde,    0x3e,    0xbe,    0x7e,    0xfe
  145. };
  146.  
  147.  
  148.  
  149.  
  150. #define ch01(index)  (buffer[chn][index])
  151. #define ch2(index)  (((FLOAT)(0.5*SQRT2))*(buffer[0][index] + buffer[1][index]))
  152. #define ch3(index)  (((FLOAT)(0.5*SQRT2))*(buffer[0][index] - buffer[1][index]))
  153.  
  154. #define ml00(f)    (window[i        ] * f(i))
  155. #define ml10(f)    (window[0x1ff - i] * f(i + 0x200))
  156. #define ml20(f)    (window[i + 0x100] * f(i + 0x100))
  157. #define ml30(f)    (window[0x0ff - i] * f(i + 0x300))
  158.  
  159. #define ml01(f)    (window[i + 0x001] * f(i + 0x001))
  160. #define ml11(f)    (window[0x1fe - i] * f(i + 0x201))
  161. #define ml21(f)    (window[i + 0x101] * f(i + 0x101))
  162. #define ml31(f)    (window[0x0fe - i] * f(i + 0x301))
  163.  
  164. #define ms00(f)    (window_s[i       ] * f(i + k))
  165. #define ms10(f)    (window_s[0x7f - i] * f(i + k + 0x80))
  166. #define ms20(f)    (window_s[i + 0x40] * f(i + k + 0x40))
  167. #define ms30(f)    (window_s[0x3f - i] * f(i + k + 0xc0))
  168.  
  169. #define ms01(f)    (window_s[i + 0x01] * f(i + k + 0x01))
  170. #define ms11(f)    (window_s[0x7e - i] * f(i + k + 0x81))
  171. #define ms21(f)    (window_s[i + 0x41] * f(i + k + 0x41))
  172. #define ms31(f)    (window_s[0x3e - i] * f(i + k + 0xc1))
  173.  
  174.  
  175.  
  176. void fft_short(
  177.     FLOAT x_real[3][BLKSIZE_s], int chn, short *buffer[2])
  178. {
  179.     short i, j, b;
  180.  
  181.     for (b = 0; b < 3; b++) {
  182.     FLOAT *x = &x_real[b][BLKSIZE_s / 2];
  183.     short k = (576 / 3) * (b + 1);
  184.     j = BLKSIZE_s / 8 - 1;
  185.     if (chn < 2) {
  186.         do {
  187.         FLOAT f0,f1,f2,f3, w;
  188.  
  189.         i = rv_tbl[j << 2];
  190.  
  191.         f0 = ms00(ch01); w = ms10(ch01); f1 = f0 - w; f0 = f0 + w;
  192.         f2 = ms20(ch01); w = ms30(ch01); f3 = f2 - w; f2 = f2 + w;
  193.  
  194.         x -= 4;
  195.         x[0] = f0 + f2;
  196.         x[2] = f0 - f2;
  197.         x[1] = f1 + f3;
  198.         x[3] = f1 - f3;
  199.  
  200.         f0 = ms01(ch01); w = ms11(ch01); f1 = f0 - w; f0 = f0 + w;
  201.         f2 = ms21(ch01); w = ms31(ch01); f3 = f2 - w; f2 = f2 + w;
  202.  
  203.         x[BLKSIZE_s / 2 + 0] = f0 + f2;
  204.         x[BLKSIZE_s / 2 + 2] = f0 - f2;
  205.         x[BLKSIZE_s / 2 + 1] = f1 + f3;
  206.         x[BLKSIZE_s / 2 + 3] = f1 - f3;
  207.         } while (--j >= 0);
  208.     } else if (chn == 2) {
  209.         do {
  210.         FLOAT f0,f1,f2,f3, w;
  211.  
  212.         i = rv_tbl[j << 2];
  213.  
  214.         f0 = ms00(ch2); w = ms10(ch2); f1 = f0 - w; f0 = f0 + w;
  215.         f2 = ms20(ch2); w = ms30(ch2); f3 = f2 - w; f2 = f2 + w;
  216.  
  217.         x -= 4;
  218.         x[0] = f0 + f2;
  219.         x[2] = f0 - f2;
  220.         x[1] = f1 + f3;
  221.         x[3] = f1 - f3;
  222.  
  223.         f0 = ms01(ch2); w = ms11(ch2); f1 = f0 - w; f0 = f0 + w;
  224.         f2 = ms21(ch2); w = ms31(ch2); f3 = f2 - w; f2 = f2 + w;
  225.  
  226.         x[BLKSIZE_s / 2 + 0] = f0 + f2;
  227.         x[BLKSIZE_s / 2 + 2] = f0 - f2;
  228.         x[BLKSIZE_s / 2 + 1] = f1 + f3;
  229.         x[BLKSIZE_s / 2 + 3] = f1 - f3;
  230.         } while (--j >= 0);
  231.     } else {
  232.         do {
  233.         FLOAT f0,f1,f2,f3, w;
  234.  
  235.         i = rv_tbl[j << 2];
  236.  
  237.         f0 = ms00(ch3); w = ms10(ch3); f1 = f0 - w; f0 = f0 + w;
  238.         f2 = ms20(ch3); w = ms30(ch3); f3 = f2 - w; f2 = f2 + w;
  239.  
  240.         x -= 4;
  241.         x[0] = f0 + f2;
  242.         x[2] = f0 - f2;
  243.         x[1] = f1 + f3;
  244.         x[3] = f1 - f3;
  245.  
  246.         f0 = ms01(ch3); w = ms11(ch3); f1 = f0 - w; f0 = f0 + w;
  247.         f2 = ms21(ch3); w = ms31(ch3); f3 = f2 - w; f2 = f2 + w;
  248.  
  249.         x[BLKSIZE_s / 2 + 0] = f0 + f2;
  250.         x[BLKSIZE_s / 2 + 2] = f0 - f2;
  251.         x[BLKSIZE_s / 2 + 1] = f1 + f3;
  252.         x[BLKSIZE_s / 2 + 3] = f1 - f3;
  253.         } while (--j >= 0);
  254.     }
  255.  
  256.     fht(x, BLKSIZE_s);
  257.     }
  258. }
  259.  
  260. void fft_long(
  261.     FLOAT x[BLKSIZE], int chn, short *buffer[2])
  262. {
  263.     short i,jj = BLKSIZE / 8 - 1;
  264.     x += BLKSIZE / 2;
  265.  
  266.     if (chn < 2) {
  267.     do {
  268.         FLOAT f0,f1,f2,f3, w;
  269.  
  270.         i = rv_tbl[jj];
  271.         f0 = ml00(ch01); w = ml10(ch01); f1 = f0 - w; f0 = f0 + w;
  272.         f2 = ml20(ch01); w = ml30(ch01); f3 = f2 - w; f2 = f2 + w;
  273.  
  274.         x -= 4;
  275.         x[0] = f0 + f2;
  276.         x[2] = f0 - f2;
  277.         x[1] = f1 + f3;
  278.         x[3] = f1 - f3;
  279.  
  280.         f0 = ml01(ch01); w = ml11(ch01); f1 = f0 - w; f0 = f0 + w;
  281.         f2 = ml21(ch01); w = ml31(ch01); f3 = f2 - w; f2 = f2 + w;
  282.  
  283.         x[BLKSIZE / 2 + 0] = f0 + f2;
  284.         x[BLKSIZE / 2 + 2] = f0 - f2;
  285.         x[BLKSIZE / 2 + 1] = f1 + f3;
  286.         x[BLKSIZE / 2 + 3] = f1 - f3;
  287.     } while (--jj >= 0);
  288.     } else if (chn == 2) {
  289.     do {
  290.         FLOAT f0,f1,f2,f3, w;
  291.  
  292.         i = rv_tbl[jj];
  293.         f0 = ml00(ch2); w = ml10(ch2); f1 = f0 - w; f0 = f0 + w;
  294.         f2 = ml20(ch2); w = ml30(ch2); f3 = f2 - w; f2 = f2 + w;
  295.  
  296.         x -= 4;
  297.         x[0] = f0 + f2;
  298.         x[2] = f0 - f2;
  299.         x[1] = f1 + f3;
  300.         x[3] = f1 - f3;
  301.  
  302.         f0 = ml01(ch2); w = ml11(ch2); f1 = f0 - w; f0 = f0 + w;
  303.         f2 = ml21(ch2); w = ml31(ch2); f3 = f2 - w; f2 = f2 + w;
  304.  
  305.         x[BLKSIZE / 2 + 0] = f0 + f2;
  306.         x[BLKSIZE / 2 + 2] = f0 - f2;
  307.         x[BLKSIZE / 2 + 1] = f1 + f3;
  308.         x[BLKSIZE / 2 + 3] = f1 - f3;
  309.     } while (--jj >= 0);
  310.     } else {
  311.     do {
  312.         FLOAT f0,f1,f2,f3, w;
  313.  
  314.         i = rv_tbl[jj];
  315.         f0 = ml00(ch3); w = ml10(ch3); f1 = f0 - w; f0 = f0 + w;
  316.         f2 = ml20(ch3); w = ml30(ch3); f3 = f2 - w; f2 = f2 + w;
  317.  
  318.         x -= 4;
  319.         x[0] = f0 + f2;
  320.         x[2] = f0 - f2;
  321.         x[1] = f1 + f3;
  322.         x[3] = f1 - f3;
  323.  
  324.         f0 = ml01(ch3); w = ml11(ch3); f1 = f0 - w; f0 = f0 + w;
  325.         f2 = ml21(ch3); w = ml31(ch3); f3 = f2 - w; f2 = f2 + w;
  326.  
  327.         x[BLKSIZE / 2 + 0] = f0 + f2;
  328.         x[BLKSIZE / 2 + 2] = f0 - f2;
  329.         x[BLKSIZE / 2 + 1] = f1 + f3;
  330.         x[BLKSIZE / 2 + 3] = f1 - f3;
  331.     } while (--jj >= 0);
  332.     }
  333.  
  334.     fht(x, BLKSIZE);
  335. }
  336.  
  337.  
  338. void init_fft(void)
  339. {
  340.     int i;
  341.  
  342.     FLOAT r = PI*0.125;
  343.     for (i = 0; i < TRI_SIZE; i++) {
  344.     costab[i*2  ] = cos(r);
  345.     costab[i*2+1] = sin(r);
  346.     r *= 0.25;
  347.     }
  348.  
  349.     /*
  350.      * calculate HANN window coefficients 
  351.      */
  352.     for (i = 0; i < BLKSIZE / 2; i++)
  353.     window[i] = 0.5 * (1.0 - cos(2.0 * PI * (i + 0.5) / BLKSIZE));
  354.     for (i = 0; i < BLKSIZE_s / 2; i++)
  355.     window_s[i] = 0.5 * (1.0 - cos(2.0 * PI * (i + 0.5) / BLKSIZE_s));
  356. }
  357.